#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/Twist.h>
#include "crc8_crc16.h"

#define VEL_TWIST_BUF_SIZE 6

#define FLOAT_ACC 4 // union存float的精度

typedef enum
{
    Line_x = 0,
    Line_y,
    Line_z,
    Ang_x,
    Ang_y,
    Ang_z,
    Total,
} vel_msg_e;

typedef enum
{
    vx = 0,
    vy,
    vth,
    chassis_Total,
} chassis_msg_e;

typedef union
{
    unsigned char c[FLOAT_ACC];
    float f;
} vel_cmd_data_u;

vel_cmd_data_u vel_cmd_data[Total];
vel_cmd_data_u chassis_data[chassis_Total];

//发给单片机的数组
unsigned char send_data[FLOAT_ACC * Total + 5];
serial::Serial ser;

void write_callback(const geometry_msgs::Twist vel_cmd_msg)
{
    //把目标速度信息写入单片机
    // ROS_INFO_STREAM("Writing to serial port" << msg->data);
    vel_cmd_data[Line_x].f = vel_cmd_msg.linear.x;
    vel_cmd_data[Line_y].f = vel_cmd_msg.linear.y;
    vel_cmd_data[Line_z].f = vel_cmd_msg.linear.z;
    vel_cmd_data[Ang_x].f = vel_cmd_msg.angular.x;
    vel_cmd_data[Ang_y].f = vel_cmd_msg.angular.y;
    vel_cmd_data[Ang_z].f = vel_cmd_msg.angular.z;

    send_data[0] = 0xA5;
    send_data[1] = 0x5A;
    for (int i = 0, k = 2; i < Total; ++i)
    {
        for (int j = 0; j < FLOAT_ACC; ++j)
        {
            send_data[k++] = vel_cmd_data[i].c[j];
        }
    }
    send_data[FLOAT_ACC * Total + 2] = 0xfe;
    append_CRC16_check_sum(send_data,sizeof(send_data));
    ser.write(send_data, sizeof(send_data));
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "serial_vel_cmd_node");
    ros::NodeHandle nh;
    //订阅cmd_vel话题，接受底盘目标数据并且串口发给单片机
    ros::Subscriber write_sub = nh.subscribe("/cmd_vel", 1000, write_callback);
    //发布read话题,发布底盘反馈数据，由odom话题接受
    ros::Publisher read_pub = nh.advertise<std_msgs::String>("r2_chassis", 1000);

    try
    {
        serial::Timeout to = serial::Timeout::simpleTimeout(100);
        serial::bytesize_t bs = serial::bytesize_t::eightbits;
        serial::stopbits_t sb = serial::stopbits_t::stopbits_one;
        serial::parity_t pt = serial::parity_none;
        serial::flowcontrol_t fc = serial::flowcontrol_t::flowcontrol_none;

        ser.setPort("/dev/ttyUSB0");
        ser.setBaudrate(115200);
        ser.setBytesize(bs);
        ser.setStopbits(sb);
        ser.setParity(pt);
        ser.setFlowcontrol(fc);
        ser.setTimeout(to);
        ser.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else
    {
        return -1;
    }
    ros::Rate loop_rate(200);
    while (ros::ok())
    {
        ros::spinOnce();
        if (ser.available())
        {
            ROS_INFO_STREAM("Reading from serial port");
            std_msgs::String result;
            result.data = ser.read(ser.available());
            ROS_INFO_STREAM("Read: " << result.data.c_str());
            read_pub.publish(result);
        }
        loop_rate.sleep();
    }
}
